#!/usr/bin/env python
#encoding:utf-8
from __future__ import print_function

import roslib
import rospy

from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
import sys, select, termios, tty
import time

msg = """
Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0,0,0),
        'o':(1,0,0,-1),
        'j':(0,0,0,1),
        'l':(0,0,0,-1),
        'u':(1,0,0,1),
        ',':(-1,0,0,0),
        '.':(-1,0,0,1),
        'm':(-1,0,0,-1),
        'O':(1,-1,0,0),
        'I':(1,0,0,0),
        'J':(0,1,0,0),
        'L':(0,-1,0,0),
        'U':(1,1,0,0),
        '<':(-1,0,0,0),
        '>':(-1,-1,0,0),
        'M':(-1,1,0,0),
        't':(0,0,1,0),
        'b':(0,0,-1,0),
    }
move_auto = {
    'go_straight':(1,0,0,0),
    'go_left':(0,0,0,1),
    'go_right':(0,0,0,-1),
    'go_back':(-1,-1,0,0)
}

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
    }


class Car_go():
    def __init__(self):
        self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        rospy.init_node('teleop_twist_keyboard', anonymous=True)
        self.speed = rospy.get_param("~speed", 0.5)
        self.turn = rospy.get_param("~turn", 1.0)
        self.twist = Twist()
        x = 0
        y = 0
        z = 0
        th = 0
        status = 0

    def shutdown(self):
        self.twist.linear.x = 0
        self.twist.linear.y = 0
        self.twist.linear.z = 0
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = 0
        self.pub.publish(self.twist)

    def go_straight(self):
        x = move_auto['go_straight'][0]
        y = move_auto['go_straight'][1]
        z = move_auto['go_straight'][2]
        th = move_auto['go_straight'][3]

        self.twist.linear.x = x * self.speed
        self.twist.linear.y = y * self.speed
        self.twist.linear.z = z * self.speed
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = th * self.turn
        self.pub.publish(self.twist)
    def go_left(self):
        x = move_auto['go_left'][0]
        y = move_auto['go_left'][1]
        z = move_auto['go_left'][2]
        th = move_auto['go_left'][3]

        self.twist.linear.x = x * self.speed
        self.twist.linear.y = y * self.speed
        self.twist.linear.z = z * self.speed
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = th * self.turn
        self.pub.publish(self.twist)

    def go_right(self):
        x = move_auto['go_right'][0]
        y = move_auto['go_right'][1]
        z = move_auto['go_right'][2]
        th = move_auto['go_right'][3]

        self.twist.linear.x = x * self.speed
        self.twist.linear.y = y * self.speed
        self.twist.linear.z = z * self.speed
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = th * self.turn
        self.pub.publish(self.twist)

    def go_back(self):
        x = move_auto['go_back'][0]
        y = move_auto['go_back'][1]
        z = move_auto['go_back'][2]
        th = move_auto['go_back'][3]

        self.twist.linear.x = x * self.speed
        self.twist.linear.y = y * self.speed
        self.twist.linear.z = z * self.speed
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = th * self.turn
        self.pub.publish(self.twist)

    def catch_pic(self):
        # self.shutdown()
        # 通过 rospy.ServiceProxy函数指定服务名来调用服务。
        # 通过rospy.wait_for_service()函数来阻塞直到服务有效。
        rospy.wait_for_service('/image_saver/save')
        self.take_pic = rospy.ServiceProxy('/image_saver/save',Empty)
        r = rospy.Rate(0.2)
        r.sleep()
        self.take_pic()

    def change_speed(self,speed,turn):
        self.speed = speed
        self.turn = turn


if __name__=="__main__":
        car = Car_go()
        for i in range(10):
            car.go_straight()